#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include "pid.h"

// 设置PID参数
static void WHT_PID_Set_Kp_Ki_Kd(WHT_PID_t *pid, float kp, float ki, float kd)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
}
// 设置PID目标值
static void WHT_PID_Set_point(WHT_PID_t *pid, float setpoint)
{
    pid->setpoint = setpoint;
}
// 清除积分值
static void WHT_Clr_Integral(WHT_PID_t* pid)
{
    pid->integral = 0.0f;
}

// PID计算函数
static float WHT_PID_Compute(WHT_PID_t *pid, float current_value, float dt)
{
    // 验证时间间隔
    if (dt <= 0.0f)
    {
        return 0.0f;  // 或返回上一次有效输出
    }
    
    // 计算误差
    float error = pid->setpoint - current_value;
    
    // 计算积分项
    pid->integral += error * dt;
    
    // 积分限幅（防饱和）
    #define MAX_INTEGRAL 100.0f
    if (pid->integral > MAX_INTEGRAL) pid->integral = MAX_INTEGRAL;
    if (pid->integral < -MAX_INTEGRAL) pid->integral = -MAX_INTEGRAL;
    
    float integral_term = pid->Ki * pid->integral;
    
    // 计算微分项（使用测量值微分）
    float derivative = 0.0f;
    if (pid->prev_measurement_initialized)
    {
        derivative = -(current_value - pid->prev_measurement) / dt;
    }
    float derivative_term = pid->Kd * derivative;
    
    // 计算PID输出
    float output = pid->Kp * error + integral_term + derivative_term;
    
    // 输出限幅
    if (output > pid->max_output)
    {
        output = pid->max_output;
        // 抗积分饱和调整（Ki不为零时）
        if (pid->Ki != 0.0f)
        {
            pid->integral = (output - pid->Kp * error - derivative_term) / pid->Ki;
        }
    } 
    else if (output < pid->min_output)
    {
        output = pid->min_output;
        if (pid->Ki != 0.0f)
        {
            pid->integral = (output - pid->Kp * error - derivative_term) / pid->Ki;
        }
    }
    
    // 保存状态
    pid->prev_error = error;
    pid->prev_measurement = current_value;
    pid->prev_measurement_initialized = true;
    
    // 安全检查
    if (isnan(output) || isinf(output))
    {
        output = (pid->min_output + pid->max_output) / 2;
    }
    
    return output;
}

// PID控制器初始化函数
static void WHT_PID_Init(WHT_PID_t *pid, float kp, float ki, float kd, float min_output, float max_output)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->setpoint = 0.0f;
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->prev_measurement = 0.0f;
    pid->min_output = min_output;
    pid->max_output = max_output;
    pid->prev_measurement_initialized = false;
}


const WHT_PID_OPS_t WHT_PID_OPS =
{
    .init         = WHT_PID_Init,
    .set_point    = WHT_PID_Set_point,
    .clr_integral = WHT_Clr_Integral,
    .set_kp_ki_kd = WHT_PID_Set_Kp_Ki_Kd,
    .compute      = WHT_PID_Compute,
};


// 示例用法
int main_PID()
{
    WHT_PID_t pid;
    float current_value = 0.0f;  // 当前值
    float output;                // 控制输出
    float dt = 0.1f;             // 时间间隔(s)
    
    // 初始化PID控制器 (Kp, Ki, Kd, 输出最小值, 输出最大值)
    WHT_PID_OPS.init(&pid, 2.0f, 0.5f, 0.1f, 0.0f, 100.0f);
    
    // 设置目标值
    WHT_PID_OPS.set_point(&pid, 50.0f);
    
    printf("PID控制示例 (目标值: 50.0)\n");
    printf("迭代\t当前值\t控制输出\n");
    
    // 模拟100次控制循环
    for (int i = 0; i < 100; i++)
    {
        // 计算控制输出
        output = WHT_PID_OPS.compute(&pid, current_value, dt);
        
        // 模拟系统响应 (简化模型: 输出直接影响当前值)
        current_value += output * dt * 0.1f;
        
        // 打印结果
        printf("%d\t%.2f\t%.2f\n", i, current_value, output);
    }
    
    return 0;
}
